#include <actionlib/client/simple_action_client.h>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <cstdlib> // Needed for rand()
#include <ctime> // Needed to seed random number generator with a time value
#include <nav_msgs/OccupancyGrid.h>
#include <queue>
#include <ros/console.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include "tuples.h"

using namespace boost::numeric::ublas;
using namespace tuples;
class SLAM {
private:
   double deltaT; // Seconds
   bool DEBUG;

   /* Matrix inversion routine.
    Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
   template<class T>
   bool InvertMatrix(const matrix<T>& input, matrix<T>& inverse) {
      typedef permutation_matrix<std::size_t> pmatrix;

      // create a working copy of the input
      matrix<T> A(input);

      // create a permutation matrix for the LU-factorization
      pmatrix pm(A.size1());

      // perform LU-factorization
      int res = lu_factorize(A, pm);
      if (res != 0)
         return false;

      // create identity matrix of "inverse"
      inverse.assign(identity_matrix<T>(A.size1()));

      // backsubstitute to get the inverse
      lu_substitute(A, pm, inverse);

      return true;
   }

public:
   SLAM() {
      deltaT = 0.2;
      DEBUG = true;
   }

   // initialize takes a vector of controls (u1:t)
   vector<position> initialize(vector<control>& controls) {
      if (DEBUG)
         std::cout << "Entering initialize()" << std::endl;

      vector<position> mu(controls.size() + 1);
      mu(0).x = 0;
      mu(0).y = 0;
      mu(0).theta = 0;

      for (unsigned int t = 0; t < controls.size(); ++t) {
         mu(t + 1).x = mu(t).x - controls(t).v / controls(t).w * sin(mu(t).theta) + controls(t).v / controls(t).w * sin(mu(t).theta + controls(t).w * deltaT);
         mu(t + 1).y = mu(t).x + controls(t).v / controls(t).w * cos(mu(t).theta) - controls(t).v / controls(t).w * cos(mu(t).theta + controls(t).w * deltaT);
         mu(t + 1).theta = controls(t).w * deltaT;
      }

      if (DEBUG)
         std::cout << "Leaving initialize()" << std::endl;
      return mu;
   }

   // linearize takes a vector of controls (u1:t), measurements (z1:t), correspondences (c1:t), position estimates (mu0:t)
   void linearize(vector<control>& uT, vector<vector<vector<double> > >& zT, vector<int> cT, vector<position>& muT, int t) {
      matrix<matrix<double> > Omega; // TODO Add some dimensions?
      vector<position> xi;

      matrix<double> p0 = zero_matrix<double>(3, 3);
      for (unsigned int i = 0; i < 3; ++i)
         p0(i, i) = std::numeric_limits<double>::max();
      Omega(0, 0) = p0;

      // Iterate over all controls u_t=(v_t w_t)^T
      for (unsigned int i = 0; i < uT.size(); ++i) {
         double uTQuotient = uT(i + 1).v / uT(i + 1).w;

         vector<double> xHat = muT(i);
         xHat(0) += -uTQuotient * sin(muT(i).theta) + uTQuotient * sin(muT(i).theta + uT(i + 1).w * deltaT);
         xHat(1) = uTQuotient * cos(muT(i).theta) - uTQuotient * cos(muT(i).theta + uT(i + 1).w * deltaT);
         xHat(2) = uT(i + 1).w * deltaT;

         matrix<double> Gt = zero_matrix<double>(3, 3);
         for (unsigned int i = 0; i < 3; ++i)
            Gt(i, i) = 1;
         Gt(0, 2) = -uTQuotient * cos(muT(i).theta) + uTQuotient * cos(muT(i).theta + uT(i + 1).w * deltaT);
         Gt(1, 2) = -uTQuotient * sin(muT(i).theta) + uTQuotient * sin(muT(i).theta + uT(i + 1).w * deltaT);

         matrix<double> GtTStep1 = trans(Gt);
         matrix<double> GtTStep2(4, 3);
         matrix<double> GtStep2(3, 4);
         for (int x = 0; x < 4; ++x) {
            for (int y = 0; y < 3; ++y) {
               if (x == 3) {
                  GtTStep2(x, y) = 1;
                  GtTStep2(y, x) = 1;
               } else {
                  GtTStep2(x, y) = GtTStep1(y, x);
                  GtStep2(y, x) = GtTStep1(y, x);
               }
            }
         }
         matrix<double> Rt(3, 3), RtInv(3, 3);
         InvertMatrix(Rt, RtInv);

         Omega(i, i) += GtTStep2 * RtInv * GtStep2;
         Omega(i + 1, i + 1) += GtTStep2 * RtInv * GtStep2;
         xi(i) += GtTStep2 * RtInv * (xHat - GtStep2 * muT(i));
         xi(i + 1) += GtTStep2 * RtInv * (xHat - GtStep2 * muT(i));
      }

      // Iterate over all measurements z_t
      // vector<vector<double> >& zT
      for (unsigned int i = 0; i < zT.size(); ++i) {
         vector<vector<double> > measurement = zT(i);
         matrix<double> Qt = zero_matrix(3, 3), QtInv(3, 3);
         Qt(0, 0) = 1; //standardDeviationR; // fuck if I know where this comes from
         Qt(1, 1) = 1; //standardDeviationPhi; // more random variables yay
         Qt(2, 2) = 1; //standarDeviationS;
         InvertMatrix(Qt, QtInv);

         //r = range to the observed landmark
         // phi = bearing to the landmark, so angle I presume relative to the robot
         //s = the signature of the landmark.. fuck if I know what this is, but apparently it can have a standard deviation
         //standard deviations are the deviations in the noise
         for (unsigned int k = 0; k < measurement.size(); k++) {
            // k-th feature corresponds to j-th landmark. Now what?
            vector<double> feature = measurement(k);
            int j = cT(k); // Chapter 6.6? k-th correspondance?

            vector<double> delta(2);
            double deltaX = muT(j).x - muT(t).x;
            double deltaY = muT(j).y - muT(t).y;
            delta(0) = muT(j).x - muT(t).x;
            delta(1) = muT(j).y - muT(t).y;
            double q = trans(delta) * delta;
            double sqrtQ = sqrt(q);

            vector<double> zHat(3);
            zHat(0) = sqrtQ;
            zHat(1) = atan2(deltaY, deltaX) - muT(t).theta;
            zHat(2) = s(j); // Hmpf!?

            matrix<double> H = zero_matrix(3, 6);
            H(0, 0) = -sqrtQ * deltaX;
            H(0, 1) = -sqrtQ * deltaY;
            H(0, 3) = sqrtQ * deltaX;
            H(0, 4) = sqrtQ * deltaY;
            H(1, 0) = deltaY;
            H(1, 1) = -deltaX;
            H(1, 2) = -q;
            H(1, 3) = -deltaY;
            H(1, 4) = deltaX;
            H(2, 5) = q;
            H /= q;

            matrix<double> measurementUpdate = trans(H) * QtInv * H; // (6,3)*(3,3)*(3,6) => (6,6)
            // Please let this work and be correct....^^
            matrix<double> incPosition = project(measurementUpdate, range(0, 3), range(0, 3)); // top left
            matrix<double> incFeatPos = project(measurementUpdate, range(0, 3), range(3, 6)); // top right
            matrix<double> incPosFeat = project(measurementUpdate, range(3, 6), range(0, 3)); // bottom left
            matrix<double> incFeature = project(measurementUpdate, range(3, 6), range(3, 6)); // bottom right

            Omega(t, t) += incPosition;
            Omega(t, j) += incFeatPos;
            Omega(j, t) += incPosFeat;
            Omega(j, j) += incFeature;
            matrix<double> a(1, 6);
            a(0, 0) = muT(t).x;
            a(0, 1) = muT(t).y;
            a(0, 2) = muT(t).theta;
            a(0, 3) = muT(j).x;
            a(0, 4) = muT(j).y;
            a(0, 5) = muT(j).s; // apparently mu has fucking s values now.. cuz why the fuck not..
            xi(t) += trans(H) * QtInv * (feature - zHat + H * a);
         }
      }
   }

//   void linearize(vector<control> u_t, vector<position> z_t, int c_t, vector<position> mu_t, int t) {
//      //yes  Iknow variables shouldt have capitals, however this is big omega so screw you :D
//      matrix<position> Omega;
//      vector<position> xi;
//      Omega(0, 0).x += std::numeric_limits<double>::max();
//      Omega(0, 0).y += std::numeric_limits<double>::max();
//      Omega(0, 0).theta += std::numeric_limits<double>::max();
//
//      vector<position> xdakje;
//      for (unsigned int i = 1; i < u_t.size() - 1; i++) {
//         xdakje(i).x = -(u_t(i).v / u_t(i).w) * sin(mu_t(i - 1).theta) + (u_t(i).v / u_t(i).w) * cos(mu_t(i - 1).theta + u_t(i).w * deltaT);
//         xdakje(i).y = (u_t(i).v / u_t(i).w) * cos(mu_t(i - 1).theta) - (u_t(i).v / u_t(i).w) * sin(mu_t(i - 1).theta + u_t(i).w * deltaT);
//         xdakje(i).theta = u_t(i).w * deltaT;
//         matrix<double, double> Gt = zero_matrix(3, 3);
//         Gt(0, 0) = 1;
//         Gt(0, 2) = -(u_t(i).v / u_t(i).w) * cos(mu_t(i - 1).theta) + (u_t(i).v / u_t(i).w) * cos(mu_t(i - 1).theta + u_t(i).w * deltaT);
//         Gt(1, 1) = 1;
//         Gt(1, 2) = -(u_t(i).v / u_t(i).w) * sin(mu_t(i - 1).theta) + (u_t(i).v / u_t(i).w) * sin(mu_t(i - 1).theta + u_t(i).w * deltaT);
//         Gt(2, 2) = 1;
//
//         matrix<double, double> GtTStep1 = trans(Gt);
//         matrix<double, double> GtTStep2(4, 3);
//         matrix<double, double> GtStep2(3, 4);
//         for (int x = 0; x < 4; x++) {
//            for (int y = 0; y < 3; y++) {
//               if (x == 3) {
//                  GtTStep2(x, y) = 1;
//                  GtTStep2(y, x) = 1;
//               } else {
//                  GtTStep2(x, y) = GtTStep1(y, x);
//                  GtStep2(y, x) = GtTStep1(y, x);
//               }
//            }
//         }
//         //I can't figure out for the love of me how to do the inverse with that package, doesnt seem to exits, not do I have
//         //the slightest fucking clue where the hell Rt comes from I think it should be a known, but still
//         matrix<double> Rt(3, 3), RtInv(3, 3);
//         InvertMatrix(Rt, RtInv);
//         // TODO +=?
//         Omega(i, i) += GtTStep2 * RtInv * GtStep2;
//         Omega(i - 1, i - 1) += GtTStep2 * RtInv * GtStep2;
//         xi(i) += GtTStep2 * RtInv * (xdakje(i) - GtStep2 * mu_t(i - 1));
//         xi(i - 1) += GtTStep2 * RtInv * (xdakje(i) - GtStep2 * mu_t(i - 1));
//
//      }
//      for (int t = 0; t < z_t.size(); t++) {
//
//         matrix<double, double> Qt = zero_matrix(3, 3);
//         Qt(0, 0) = standardDeviationR;
//         Qt(1, 1) = standardDeviationPhi;
//         Qt(2, 2) = standarDeviationS;
//
//         //r = range to the observed landmark
//         // phi = bearing to the landmark, so angle I presume relative to the robot
//         //s = the signature of the landmark.. fuck if I know what this is, but apparently it can have a standard deviation
//         //standard deviations are the deviations in the noise
//         for (int i = 0; i < z_t_i.size(); i++) {
//            int j = c_i_t; //I have no idea what this is or where I'm supposed to get it from.
//
//            matrix<double, double> delta(1, 2);
//            double deltaX = mu_t(j).x - mu_t(t).x;
//            double deltaY = mu_t(j).y - mu_t(t).y;
//            delta(0, 1) = mu_t(j).x - mu_t(t).x;
//            delta(0, 2) = mu_t(j).y - mu_t(t).y;
//            q = trans(delta) * delta;
//
//            matrix<double, double> zDakje_i_t(1, 3);
//            zDakje_i_t(0, 1) = sqrt(q);
//            zDakje_i_t(0, 2) = atan2(deltaY, deltaX) - mu_t[t].theta;
//            matrix<double, double> H = zero_matrix(3, 6);
//            H(0, 0) = -sqrt(q * deltaX);
//            H(0, 1) = -sqrt(q * deltaY);
//            H(0, 3) = +sqrt(q * deltaX);
//            H(0, 4) = +sqrt(q * deltaY);
//            H(1, 0) = deltaY;
//            H(1, 1) = -deltaX;
//            H(1, 2) = -q;
//            H(1, 3) = -deltaY;
//            H(1, 4) = deltaX;
//            H(2, 5) = q;
//            H = 1 / q * H;
//
//            matrix<double, double> QtInv(3, 3);
//            InvertMatrix(Qt, QtInv);
//            Omega(t, t) += trans(H) * QtInv * H;
//            Omega(j, j) += trans(H) * QtInv * H;
//            matrix<double, double> a(1, 6);
//            a(0, 0) = mu_t(t).x;
//            a(0, 1) = mu_t(t).y;
//            a(0, 2) = mu_t(t).theta;
//            a(0, 3) = mu_t(j).x;
//            a(0, 4) = mu_t(j).y;
//            a(0, 5) = mu_t(j).s; // apparently mu has fucking s values now.. cuz why the fuck not..
//            xi(t) += trans(H) * QtInv * (z_i_t - zDakje_i_t + H * a);
//         }
//      }
//   }

   void reduce(int omega, int xsi) {
      // TODO Change data type & use ptr/reference?
   }

   void solve(int omega_tilde, int xsi_tilde, int omega, int xsi) {
      // TODO Change data type & use ptr/reference?
   }

   void known_correspondence(int u_t, int z_t, int c_t) {
      // TODO Change data type & use ptr/reference?
   }

   void matrixInversionExample() {
      matrix<double> A(3, 3), Z(3, 3);
      A(0, 0) = 1;
      A(1, 0) = 5;
      A(2, 0) = 6;
      A(0, 1) = 2;
      A(1, 1) = 1;
      A(2, 1) = 7;
      A(0, 2) = 3;
      A(1, 2) = 4;
      A(2, 2) = 1;
      InvertMatrix(A, Z);
      std::cout << "A=" << A << std::endl << "Z=" << Z << std::endl;
   }
};

int main(int argc, char **argv) {
   // Initiate new ROS node named "turtle_map_processor"
   ros::init(argc, argv, "turtle_slam");
   ros::NodeHandle n;

   SLAM slam;

   // Some random value test for initialize()
   //   vector<control> input(2);
   //   input(0).v = 4;
   //   input(0).w = 2;
   //   input(1).v = 8;
   //   input(1).w = 1;
   //
   //   vector<position> result = slam.initialize(input);
   //   for (vector<position>::iterator it = result.begin(); it != result.end(); ++it)
   //      std::cout << (*it).x << " " << (*it).y << " " << (*it).theta << std::endl;

   // ----------------------
   // Some random example for matrix inversion
   slam.matrixInversionExample();

   ros::spin();
   return 0;
}
